/**********************************************************************************************
***********************************************************************************************
***  File:			Kalman.h
***	 Author:		José Oniram / Elias Rachid / Jefferson / George Andrew
***	 Contents:		Kalman header file.
***********************************************************************************************
**********************************************************************************************/

#include <listas.h>
#include <math.h>
#include <cstdlib>

#define alpha_x 0.1 
#define alpha_y 0.1
#define alpha_theta 0.1

#define sigma_r 0.1
#define sigma_phi 0.1
#define sigma_s 0.01

#define num_land 12

// Protótipos da Funções presentes no Kalman.cpp

int test (void);
void SLAM(PGMATRIX Matriz_Mediat, PGMATRIX Matriz_Covt, double controle[], int signature[], list<lista_mapa> *Lista_M, list<lista_sensor> *Lista_S, double d_t);

// Motion Update

void Gera_Matriz_Fx(PGMATRIX Matriz_Fx);
void Atualiza_Media_Controle(double media_t[], double controle[], double theta_robo, double d_t);
void Gera_Matriz_Gt(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Fx, double controle[], double theta_robo, double d_t);
void Gera_Matriz_Rt(PGMATRIX Matriz_Rt);
void Atualiza_Covariancia_Controle(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Fx, PGMATRIX Matriz_Rt, PGMATRIX Matriz_Covt);

// Measurement Update

void Gera_Matriz_Qt(PGMATRIX Matriz_Qt);
void Landmark_First_Time(double media_landmark[], double media_t[], double z_landmark[]);
void Gera_Delta_e_Q(double dx_dy_q[], double media_t[], double media_landmark[]);
void Gera_Matriz_Ht(PGMATRIX Matriz_Ht, double dx_dy_q[], int j);

void Gera_Matriz_Kt(PGMATRIX Matriz_Kt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_Qt, PGMATRIX Matriz_Covt);

void Atualiza_Media_Covariancia_Sensores(double media_t[], double dx_dy_q[], double z_landmark[], PGMATRIX Matriz_Kt, PGMATRIX Matriz_Covt, PGMATRIX Matriz_Ht, list<lista_mapa> *Lista_Mapa);

